#ifndef ROBORTS_DECISION_COVER_BEHAVIOR_H
#define ROBORTS_DECISION_COVER_BEHAVIOR_H

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <tf/tf.h>
#include "io/io.h"
#include <iostream>
#include "../blackboard/blackboard.h"
#include "../executor/chassis_executor.h"
#include "../behavior_tree/behavior_state.h"
#include "../proto/decision.pb.h"
#include "../../roborts_base/include/communication.h"
#include "roborts_msgs/multi_car.h"
#include "roborts_msgs/single_car.h"
#include "math.h"
namespace roborts_decision {
	class SupportBehavior{
		public:
		SupportBehavior(ChassisExecutor* &chassis_executor, Blackboard* &blackboard, const std::string & proto_file_path) 
		: chassis_executor_(chassis_executor), blackboard_(blackboard){
			ros::NodeHandle n;
			my_pose_publish = n.advertise<geometry_msgs::PoseStamped>("support_position",1);
			enemy_pose_publish = n.advertise<geometry_msgs::PoseStamped>("enemy_position",1);
		}

		void Run(){
			auto executor_state = Update();
			if (executor_state != BehaviorState::RUNNING) {
				geometry_msgs::PoseStamped enemy_aim=blackboard_->GetNewEnemyCar();
				geometry_msgs::PoseStamped friend_place=blackboard_->getfriendpose();
				my_place=blackboard_->GetRobotMapPose();
				enemy_aim.header.frame_id = "map";
				enemy_pose_publish.publish(enemy_aim);
				double dx = enemy_aim.pose.position.x - friend_place.pose.position.x;
				double dy = enemy_aim.pose.position.y - friend_place.pose.position.y;

				double yaw = atan2(dy,dx)+M_PI;    //calculate the angle of friend and enemy;
				int depth = 0;
				auto my_pose = select_position(enemy_aim,yaw,depth);
				if (std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) <=0.2)
				{
					// chassis_executor_->Cancel();
					return;
				}
                std::cout<<my_pose<<std::endl;
				// chassis_executor_->Execute(my_pose, Behaviortype::CHASE);
				my_pose_publish.publish(my_pose);
			}
		}

		void Cancel() {
			chassis_executor_->Cancel();
		}

		BehaviorState Update() {
			return chassis_executor_->Update();
		}

		~SupportBehavior()=default;

		private:
		ChassisExecutor* const chassis_executor_;
		Blackboard* const blackboard_;
		geometry_msgs::PoseStamped my_place;
		ros::Publisher my_pose_publish;
		ros::Publisher enemy_pose_publish;
		const roborts_costmap::Costmap2D* costmap_2d_;

		geometry_msgs::PoseStamped select_position(geometry_msgs::PoseStamped& enemy_pose,double yaw,int depth)
		{
			if(depth>3)
			{
				enemy_pose.header.frame_id = "map";
				return enemy_pose;
			}
			int samples = 5;
			double resolution = 0.15;   
			double my_distance = 100;
			geometry_msgs::PoseStamped my_pose;
			for(int i=0;i<samples;i++)
			{
				double sample_yaw = yaw - resolution/2*samples+i*resolution;
				auto current_max = max_distance(enemy_pose,sample_yaw);
				std::cout<<current_max<<std::endl;
				if(current_max<2)
				{
					continue;
				}
				else if(current_max>=2)
				{
					geometry_msgs::PoseStamped cad_pose = enemy_pose;
					cad_pose.pose.position.x -=2*cos(sample_yaw);
					cad_pose.pose.position.y -= 2*sin(sample_yaw);
					auto cad_dis = calculate_distance(cad_pose,my_place);
					if(my_distance>cad_dis)
					{
						my_distance = cad_dis;
						my_pose = cad_pose;
						my_pose.pose.orientation =  tf::createQuaternionMsgFromRollPitchYaw(0,0,sample_yaw);
					}
				}
				else if(current_max>2.5)
				{
					geometry_msgs::PoseStamped cad_pose = enemy_pose;
					cad_pose.pose.position.x -=2.5*cos(sample_yaw);
					cad_pose.pose.position.y -= 2.5*sin(sample_yaw);
					auto cad_dis = calculate_distance(cad_pose,my_place);
					if(my_distance>cad_dis)
					{
						my_distance = cad_dis;
						my_pose = cad_pose;
						my_pose.pose.orientation =  tf::createQuaternionMsgFromRollPitchYaw(0,0,sample_yaw);
					}
				}
			}
			if(my_distance!=100)
			{
				my_pose.header.frame_id = "map";
				return my_pose;
			}
			else{
				return select_position(enemy_pose,yaw+1.57,depth);
				// enemy_pose.header.frame_id = "map";
				// return enemy_pose;
			}
		}

		double calculate_distance(geometry_msgs::PoseStamped& pose1,geometry_msgs::PoseStamped& pose2 )
		{
			double dx = pose1.pose.position.x - pose2.pose.position.x;
			double dy = pose1.pose.position.y - pose2.pose.position.y;
			double  dis = sqrt(dx*dx+dy*dy);
			return dis;
		}

		bool PointAvaible(double mx,double my)
		{
			if(mx>0.5&&mx<7.5&&my>0.5&&my<4)
				return true;
			else
				return false;
		}

		double max_distance(geometry_msgs::PoseStamped& enemy_pose,double yaw)
		{
			costmap_2d_ = blackboard_->GetCostMap2D();
			auto global_costmap_ = blackboard_->GetGlobalCostMap();
			int i=0;
			auto my_pose_x = enemy_pose.pose.position.x-1.5*cos(yaw);
			auto my_pose_y = enemy_pose.pose.position.y-1.5*sin(yaw);
			while(i<30)
			{
				my_pose_x -= 0.1*cos(yaw);
				my_pose_y -= 0.1*sin(yaw);
				unsigned int mx,my;
				if(costmap_2d_->World2Map(my_pose_x,my_pose_y,mx,my))
				{
					if(costmap_2d_->GetCost(mx,my)!=0)
					{
						return 0.1*i+1.4;
					}
				}
				else
				{
					if(i == 0)
						return 0;
					else
						return i*0.1+1.4;
				}
				i++;
			}
			return i*0.1+1.4;
		}
	};
}

#endif
